include "map_builder.lua"
include "trajectory_builder.lua"

options = {
    map_builder = MAP_BUILDER,  -- 使用 MAP_BUILDER 模块
    trajectory_builder = TRAJECTORY_BUILDER,  -- 使用 TRAJECTORY_BUILDER 模块
    map_frame = "map",  -- 地图坐标系的名称
    tracking_frame = "gyro_link",  -- 机器人基础坐标系的名称
    published_frame = "base_link",  -- 机器人发布里程计信息的坐标系
    odom_frame = "odom",  -- 里程计坐标系的名称
    provide_odom_frame = true,  -- 是否由 Cartographer 提供里程计坐标系
    publish_frame_projected_to_2d = true,  -- 将发布的位姿投影到2D平面
    use_odometry = true,  -- 是否使用外部里程计数据
    use_nav_sat = false,  -- 是否使用卫星导航数据
    use_landmarks = false,  -- 是否使用地标进行定位
    num_laser_scans = 1,  -- 激光扫描传感器的数量
    num_multi_echo_laser_scans = 0,  -- 多回波激光扫描传感器的数量
    num_subdivisions_per_laser_scan = 1,  -- 每次激光扫描的子扫描数
    num_point_clouds = 0,  -- 点云传感器的数量
    lookup_transform_timeout_sec = 0.2,  -- 查找坐标变换的超时时间
    submap_publish_period_sec = 0.3,  -- 发布子地图的时间间隔
    pose_publish_period_sec = 5e-3,  -- 发布位姿的时间间隔
    trajectory_publish_period_sec = 30e-3,  -- 发布轨迹的时间间隔
    rangefinder_sampling_ratio = 0.3,  -- 使用所有激光数据
    odometry_sampling_ratio = 0.5,  -- 使用所有里程计数据
    fixed_frame_pose_sampling_ratio = 1.0,  -- 固定坐标系位姿数据的采样比例
    imu_sampling_ratio = 0.5,  -- 使用所有IMU数据
    landmarks_sampling_ratio = 1.,
  }
  
  -- 设置 Map Builder 使用 2D 模式
  MAP_BUILDER.use_trajectory_builder_2d = true

-- 设置 2D 轨迹构建器参数
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1     -- 累积的激光数据数量
TRAJECTORY_BUILDER_2D.use_imu_data = true  -- 确保使用IMU数据
TRAJECTORY_BUILDER_2D.min_range = 0.3                     -- 减小最小范围以获取更多近距离细节
TRAJECTORY_BUILDER_2D.max_range = 8.0                    -- 限制最大范围以提高精度
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 2.0       -- 减小缺失数据射线长度
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.01           -- 1cm体素滤波
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.5      -- 减小时间窗口
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.2  -- 稍微增加位移阈值
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = 0.1  -- 稍微增加角度阈值
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.5          -- 添加距离限制

TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.2
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 300  -- 适当减少点云要求
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 8.0      -- 减小范围

TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 0.9  -- 回环闭合自适应体素滤波器的最大长度
TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100  -- 回环闭合自适应体素滤波器的最小点数
TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_range = 50.0  -- 回环闭合自适应体素滤波器的最大范围

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false  -- 是否使用在线相关性扫描匹配
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1  -- 实时相关性扫描匹配的线性搜索窗口
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(5.0)  -- 实时相关性扫描匹配的角度搜索窗口

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 20.0
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 100.0
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 200.0
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 50
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.num_threads = 1

POSE_GRAPH.optimization_problem.huber_scale = 1e3  -- Huber 损失函数的尺度
POSE_GRAPH.optimize_every_n_nodes = 90                   -- 减少优化频率
POSE_GRAPH.global_sampling_ratio = 0.003                 -- 提高全局采样率
POSE_GRAPH.global_constraint_search_after_n_seconds = 10.  -- 多少秒后开始搜索全局约束
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3  -- 降低采样率以减少工作队列
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.0
POSE_GRAPH.constraint_builder.min_score = 0.55      -- 降低最小分数以获得更多约束
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.6
POSE_GRAPH.matcher_translation_weight = 5e2  -- 匹配器的平移权重
POSE_GRAPH.matcher_rotation_weight = 1.6e3  -- 匹配器的旋转权重
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 50
POSE_GRAPH.optimization_problem.ceres_solver_options.num_threads = 4  -- 使用多线程
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 7.0
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(30.0)
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth = 7

POSE_GRAPH.constraint_builder.loop_closure_translation_weight = 1.1e4  -- 回环检测平移权重
POSE_GRAPH.constraint_builder.loop_closure_rotation_weight = 1e5       -- 回环检测旋转权重

-- 扫描匹配优化参数
POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight = 1e5
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e5

-- 回环检测的分辨率
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.01  -- 1cm地图分辨率

-- 子图大小
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 100  -- 增加子图数据量
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.70
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.35
TRAJECTORY_BUILDER_2D.max_range = 10.0  -- 增加最大范围
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 10.0

POSE_GRAPH.max_num_final_iterations = 100                 -- 增加最终优化迭代次数

return options
